/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <algorithm>

#include "Terrain.h"
#include "World.h"
#include "mathutils.h"
#include "Material.h"
#include "owndebug.h"
#include "MaterialHandler.h"
#include "gimiprotocoltypes.h"
#include "TGIMI_PROTOCOL_GRID.h"

#include "MapTypes.hpp"
#include "MapData.hpp"
#include "binbag.h"


/*
 * calculates indexed vertex format for heightmap data
 * 
 * Layout of triangles:
 * *=heightmap sample
 * 1,2,3=index numbers
 *  
 *  1    2
 * 1*---*---*---*...
 *  | \ |3  |   |
 * 2*---*---*---*...
 *  |   |   |   |
 *  *---*---*---*...
 */
TGroundModelBlock::TGroundModelBlock(float x, float y, float resolution)
  :x(x),
   y(y),
   resolution(resolution),
   Trimesh(NULL)
{
 
  //dPrint(1,"constructor");
  for (int xx=0; xx<GROUND_MODEL_BLOCK_SIZE; xx++)
    {
      //dPrint(1,"xx");
      for (int yy=0; yy<GROUND_MODEL_BLOCK_SIZE; yy++)
        {
          //dPrint(1,"yy");
          //NOTICE: polygon direction (winding) does matter!
          indexData[(yy*(GROUND_MODEL_BLOCK_SIZE)+xx)*6]=(yy*(GROUND_MODEL_BLOCK_SIZE+1)+xx);
          indexData[(yy*(GROUND_MODEL_BLOCK_SIZE)+xx)*6+1]=((yy)*(GROUND_MODEL_BLOCK_SIZE+1)+xx+1);
          indexData[(yy*(GROUND_MODEL_BLOCK_SIZE)+xx)*6+2]=((yy+1)*(GROUND_MODEL_BLOCK_SIZE+1)+xx+1);

          indexData[(yy*(GROUND_MODEL_BLOCK_SIZE)+xx)*6+3]=(yy*(GROUND_MODEL_BLOCK_SIZE+1)+xx);
          indexData[(yy*(GROUND_MODEL_BLOCK_SIZE)+xx)*6+4]=((yy+1)*(GROUND_MODEL_BLOCK_SIZE+1)+xx+1);
          indexData[(yy*(GROUND_MODEL_BLOCK_SIZE)+xx)*6+5]=((yy+1)*(GROUND_MODEL_BLOCK_SIZE+1)+xx);
      
        }
    }
  for (int xx=0; xx<GROUND_MODEL_BLOCK_SIZE+1; xx++)
    {
      for (int yy=0; yy<GROUND_MODEL_BLOCK_SIZE+1; yy++)
        {
          //set vertices
          vertexData[(yy*(GROUND_MODEL_BLOCK_SIZE+1)+xx)*3]=1.0*xx*resolution;
          vertexData[(yy*(GROUND_MODEL_BLOCK_SIZE+1)+xx)*3+1]=0.0;
#warning "Using inverted y-axis and heading"
          vertexData[(yy*(GROUND_MODEL_BLOCK_SIZE+1)+xx)*3+2]=-1.0*yy*resolution;
        }
    }

  //dPrint(1,"constructor end");
}

TGroundModelBlock::~TGroundModelBlock()
{
  if (Trimesh) delete Trimesh;
}

/**********************************************/

CTerrain::CTerrain(CWorld *World, char *fileName, std::string name, bool wrap)
  :CObject(World,NULL),
   name(name),
   type(TERRAIN_TYPE_HMAP),
   //data(NULL),
   //hmData(NULL),
   wrap(wrap),
   gimi(GIMI_DEFAULTQUEUESIZE,"FSRSim Terrain"),
#if ! defined(NO_MAP)
   mapClient(&gimi,0),
   seq(-1),
#endif
   //gimi(GIMI_DEFAULTQUEUESIZE, "FSRSim ground model"),
   /*hmListInited(false),
     width(20),
     depth(20),
     originW(10),
     originD(10),*/
   reso(-1),
   /*wSamples(201),
     dSamples(201),*/
   maxHeight(-10000),
   minHeight(10000),
   hmResolution(-1),
   Plane(NULL),
   //vertices(NULL),
   //indices(NULL),
   updateCounter(0)
  /*   iLineMapGimi(GIMI_DEFAULTQUEUESIZE,"FSRSim Terrain"),
   iLineMapMSC(&iLineMapGimi),
   iLineMapServer(&iLineMapMSC, 0, "Linemap Server")*/
{
  material= new TMaterial(DEFAULT_MATERIAL);
  material->look.color[0]=0.7;
  material->look.color[1]=0.7;
  material->look.color[2]=0.5;

  material->surface.mu=100;
  material->surface.soft_erp=0.8;
  material->surface.soft_cfm=1e-4;

  loadFile(fileName); //read data from file
  dPrint(1,"filename is %s",fileName);

}

CTerrain::CTerrain(CWorld *World, float x1, float x2, float y1, float y2)
  :CObject(World,NULL),
   type(TERRAIN_TYPE_PLANE)
#if ! defined(NO_MAP)
  ,gimi(GIMI_DEFAULTQUEUESIZE,"FSRSim Terrain"),
   mapClient(&gimi,0)
  /*   iLineMapGimi(GIMI_DEFAULTQUEUESIZE,"FSRSim Terrain"),
   iLineMapMSC(&iLineMapGimi),
   iLineMapServer(&iLineMapMSC, 0, "Linemap Server")*/

#endif
  //data(NULL)
  //hmData(NULL)
{
  this->x1=std::min(x1,x2);
  this->x2=std::max(x1,x2);
#warning "NOTE: Using inverted y-axis and heading"
  this->y1=std::min(-y1,-y2);
  this->y2=std::max(-y1,-y2);

  width=fabs(x1-x2);
  depth=fabs(y1-y2);


  material= new TMaterial();

  //material->surface.soft_erp=0.0;
  //material->surface.soft_cfm=0.0;

  material->look.color[0]=0.7;
  material->look.color[1]=0.8;
  material->look.color[2]=0.7;
  Plane=new CTexturedPlane(this->x1,this->y1,this->x2,this->y2,this,SpaceID,material);
  //Plane->LoadTexture("models/textures/concrete.png");

  //GeomID=dCreatePlane (SpaceID, 0, 1, 0, 0);
  //SimWorld->MaterialHandler->Add(this->GeomID,material);
}

CTerrain::~CTerrain()
{
  delete material;

}

void CTerrain::Activate()
{
  connectGIMI();
  
#if ! defined(NO_MAP)

  MaCI::Map::CMapData mapData;
  if (mapClient.GetMap(mapData,2000))
    {
      const MaCI::Map::TMapHeader *header;
    
    
      header=mapData.GetMapHeader();
    
      if (header)
        {
          hmResolution=header->gridDataResolution;
        }
    
      updateMap(mapData);
    
    } else {
      dPrint(1,"Couldn't get map from server!!");
      
      //Startin linemap server
      //      StartLineMapServer("../MaCI/machines/Motti/default_Motti.map");
    }

#endif
  
}


void CTerrain::Draw()
{


  //const dReal *R, *P;
  //float M[16];

  if (type==TERRAIN_TYPE_PLANE)
    {
      GLfloat fadeColor[4]={0,0,0,0};

      GLfloat h=PEDESTAL_HEIGHT;

      glGetFloatv(GL_COLOR_CLEAR_VALUE,fadeColor);

      glPushMatrix();
      glPushAttrib(GL_ALL_ATTRIB_BITS);

      //draw plane and the pedestal. pedestal's base fades to background
      glBegin(GL_QUADS);

      glNormal3f(0,0,-1);
      glColor3f(material->look.color[0],material->look.color[1],material->look.color[2]);
      glVertex3f(x2,0,y2);
      glVertex3f(x1,0,y2);
      glColor3f(fadeColor[0],fadeColor[1],fadeColor[2]);
      glVertex3f(x1,-h,y2);
      glVertex3f(x2,-h,y2);

      glNormal3f(0,0,1);
      glColor3f(material->look.color[0],material->look.color[1],material->look.color[2]);
      glVertex3f(x1,0,y1);
      glVertex3f(x2,0,y1);
      glColor3f(fadeColor[0],fadeColor[1],fadeColor[2]);
      glVertex3f(x2,-h,y1);
      glVertex3f(x1,-h,y1);

      //        glNormal3f(0,1,0);
      glColor3f(material->look.color[0],material->look.color[1],material->look.color[2]);
      //        glVertex3f(x2,0,y1);
      //        glVertex3f(x1,0,y1);
      //        glVertex3f(x1,0,y2);
      //        glVertex3f(x2,0,y2);

      glNormal3f(-1,0,0);
      glVertex3f(x2,0,y2);
      glColor3f(fadeColor[0],fadeColor[1],fadeColor[2]);
      glVertex3f(x2,-h,y2);
      glVertex3f(x2,-h,y1);
      glColor3f(material->look.color[0],material->look.color[1],material->look.color[2]);
      glVertex3f(x2,0,y1);

      glNormal3f(1,0,0);
      glVertex3f(x1,0,y2);
      glVertex3f(x1,0,y1);
      glColor3f(fadeColor[0],fadeColor[1],fadeColor[2]);
      glVertex3f(x1,-h,y1);
      glVertex3f(x1,-h,y2);

      glEnd();

      glPopAttrib();
      glPopMatrix();
    
      Plane->Draw();
    

      if(SimWorld->GetDrawGridMode())
        {
          drawGrid();
        }


    } else {
    
      gmbMapIter i;
      for (i=groundBlocks.begin();i!=groundBlocks.end();i++)
        i->second->Trimesh->Draw();
    
    }
}

void CTerrain::drawGrid()
{
  if (type==TERRAIN_TYPE_PLANE)
    {
      //GLfloat w=width/2,h=PEDESTAL_HEIGHT,l=depth/2;

      GLfloat h=0.01;

      glPushMatrix();
      glPushAttrib(GL_ALL_ATTRIB_BITS);

      //glDisable(GL_DEPTH_TEST);

      glLineWidth(2.0);
      glBegin(GL_LINES);

      //draw x and y axes
      glColor3f(1.0,0.5,0.5);
      glVertex3f(x1,h,0);
      glVertex3f(x2,h,0);

      glColor3f(0.5,1.0,0.5);
      glVertex3f(0,h,y1);
      glVertex3f(0,h,y2);

      glColor3f(0.6,0.7,0.6);
      //draw + and - signs
#warning "NOTE: Using inverted y-axis and heading"
      //+
      glVertex3f(0.4,h,-0.5);
      glVertex3f(0.6,h,-0.5);
      glVertex3f(0.5,h,-0.4);
      glVertex3f(0.5,h,-0.6);
      //-
      glVertex3f(-0.4,h,0.5);
      glVertex3f(-0.6,h,0.5);

      glEnd();

      glLineWidth(1.0);
      //glBegin(GL_LINES);
      //        for (float i=1; i<width/2; i++)
      //        {
      //          glVertex3f(i,h,-l);
      //          glVertex3f(i,h,l);
      //
      //          glVertex3f(-i,h,-l);
      //          glVertex3f(-i,h,l);
      //        }
      //
      //        for (float i=1; i<depth/2; i++)
      //        {
      //          glVertex3f(w,h,-i);
      //          glVertex3f(-w,h,-i);
      //
      //          glVertex3f(w,h,i);
      //          glVertex3f(-w,h,i);
      //        }
      //
      //      glEnd();

      float x=ceil(std::min(x1,x2));
      float xx=floor(std::max(x1,x2));
      float y=ceil(std::min(y1,y2));
      float yy=floor(std::max(y1,y2));

      glBegin(GL_LINES);
      for (float i=x; i<xx; i++)
        {
          glVertex3f(i,h,yy);
          glVertex3f(i,h,y);
        }

      for (float i=y; i<yy; i++)
        {
          glVertex3f(x,h,i);
          glVertex3f(xx,h,i);
        }

      glEnd();

      //glEnable(GL_DEPTH_TEST);

      glPopAttrib();
      glPopMatrix();
    }
}

void CTerrain::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{
}

void CTerrain::Update(float StepSize)
{

  if (updates.size())
    {
      //dPrint(1,"sdfsdf");
    
      for (std::vector<TUpdate>::iterator i=updates.begin();i!=updates.end();i++)
        {
          updateGroundModel(i->x, i->y, i->samples_x, i->samples_y, reso, i->data);
      
          //free(i->data);
          delete[] i->data;
        
        }
      updates.clear();
    }
  
#if ! defined(NO_MAP)
  MaCI::Map::CMapData mapData;
  if (mapClient.GetMapUpdateEvent(mapData,&seq,0))
    {

      updateMap(mapData);
    }
#endif
  
}



/*
void CTerrain::StartLineMapServer(char* aMapName)
{
  
  // Starting linemap server here
  // TODO IMPLEMENT
  using namespace MaCI::Map;

  // GIMnet parameters
  std::string gimnetAP = "localhost";
  int gimnetAPPort = 50002;
  std::string gimnetName = "LineMapServer";
  std::string macigroup = "Motti.PbV_0";
  std::string linemapfile = aMapName;

  
  // Objects
  using namespace MaCI;
  bool run = true;
  //  gimi::GIMI g;
  //MaCICtrl::CMaCICtrlServer msc(&g);
  //Map::CMapServer ms(&msc, 0, "Linemap Server"); 

  
  std::string instancename="imap";

  // Set macigroup
  //  iLineMapMSC.SetGroupName(macigroup);
  
  int r;
  if ( (r = iLineMapGimi.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    printf("ABORT 1\n");
    abort();
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
		
        iLineMapServer.SetInterfaceInstanceName(instancename);
    
    MaCI::EMaCIError e;
    if ( (e = iLineMapServer.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open MapServer instance! (%s)",
	     GetErrorStr(e).c_str());
      printf("ABORT 2\n");
      abort();
      
    } 
    else {
      //////////////////////////////////
      ////// Execute test section ///////
      
      dPrint(1,"Opened MapServer instance.");
      
      float reso=1.0; //hmap resolution
      int samples;
      int number=0; 
          
      gim::binbag::CBinBag binbag;
      CMapData MapData;
      TMapHeader MapHeader;
      TMapUpdateHeader UpdateHeader;
      TMapLine MapLine;
			
      MapData.EncodeTo(&binbag);
			
      lnm::TLineMap map;
      if(map.readMapFile(linemapfile.c_str(),1.0)<0){
        dPrint(1,"Unable to open map file '%s'. Check that you gave proper file with -f parameter");
        printf("UNABLE TO OPEN MAP FILE\n");
        abort();
      }
			
      MapHeader.gridDataResolution=reso;
      

      if (!MapData.SetMapHeader(MapHeader)) dPrint(1,"Failed to set map header");
			
      for(int i=0;i<map.NumOfLines;i++){
        MapLine.x1=map.Lines[i].x1;
        MapLine.y1=map.Lines[i].y1;
        MapLine.x2=map.Lines[i].x2;
        MapLine.y2=map.Lines[i].y2;   
        if (!MapData.AddMapLine(MapLine)) { dPrint(1,"Failed to add line"); 
        } else { dPrint(10,"Added line"); }
      }
			
      //set the map to servers current map
      if (!iLineMapServer.SetMap(MapData))
        {
          dPrint(1,"Failed to set map");
          printf("FAILED TO SET MAP!");
          abort();
        }
      




    }
  }
      
  // LINEMAP SERVER STUFF ENDS HERE







}
*/

void CTerrain::loadFile(char* fname)
{


  FILE *f;

  float originW, originD;
  
  f = fopen(fname,"rt");
  if(f == NULL){
    dPrint(1,"Couldn't load file.");
    exit(0);
  }
  dPrint(1,"Loaded file %s",fname)
    fscanf(f,"size_x:%f\n",&width);
  fscanf(f,"size_y:%f\n",&depth);
  fscanf(f,"reso:%f\n",&this->reso);
  fscanf(f,"orig_x:%f\n",&originW);
  fscanf(f,"orig_y:%f\n",&originD);

  dPrint(1,"size_x:%f\n",width);
  dPrint(1,"size_y:%f\n",depth);
  dPrint(1,"reso:%f\n",this->reso);
  dPrint(1,"orig_x:%f\n",originW);
  dPrint(1,"orig_y:%f\n",originD);

  int wSamples=lrint(width/reso+1.0)-1;
  int dSamples=lrint(depth/reso+1.0)-1;

  originW*=reso;
  originD*=reso;
  
  //  indexCount=(wSamples-1)*(dSamples-1)*2*3; //2 triangles per heightmap point and 3 vertices per triangle
  //  vertexCount=wSamples*dSamples;

  //dPrint(1,"vertices:%d indices:%d",vertexCount,indexCount);

  dPrint(1,"allocating data, wSamples=%d dSamples=%d, product=%d",wSamples,dSamples,wSamples*dSamples);
  
  float *data=NULL;
  data=new float[wSamples*dSamples];

  if (data==NULL)
    {
      dPrint(1,"Out of memory.");
      exit(0);
    }

  float val;

  for(int i=0;i<dSamples;i++)
    {
      //read a line
      for(int j=0;j<wSamples;j++)
        {
          fscanf(f,"%f ", &val);

          data[i*wSamples+j]=val;

        }
      fscanf(f,"\n");
    }

  //  dPrint(1,"Created vertices");

  fclose(f);

  //updateGroundModel(0+originW,0-originD,width,depth,reso,data);
  updateGroundModel(0+originW,0-originD,wSamples,dSamples,reso,data);

      
  delete [] data;
	
}

/*void CTerrain::updateGroundModel(const float aPosX, const float aPosY, 
  const float aWidth, const float aDepth,
  const float aReso, const float* aHmData)*/
void CTerrain::updateGroundModel(const float aPosX, const float aPosY, 
                                 const int data_samples_x, const int data_samples_y,
                                 const float aReso, const float* aHmData)
{
  /*
   * ground model is divided into GROUND_MODEL_BLOCK_SIZE wide blocks. 
   * when new data is given, only affected blocks are changed  
   * 
   */
  if (reso<0)
    reso = aReso; //use initial resolution for all data
  
  //dPrint(1,"updating model");
  
  //TODO: WTF
  //int data_samples_x=lrint(aWidth/reso+1);
  //int data_samples_y=lrint(aDepth/reso+1);
  //dPrint(1,"w=%f h=%f reso=%f d/reso=%f (%d)",aWidth, aDepth, reso, aDepth/reso,(int)(aWidth/reso));
  //dPrint(1,"samples x:%d y:%d",data_samples_x,data_samples_y);
  
  float aWidth=data_samples_x*reso; 
  float aDepth=data_samples_y*reso;
  
  //ground block limits that new data is located within
  int start_block_x =(int) floor(aPosX/reso/GROUND_MODEL_BLOCK_SIZE);
  int start_block_y =(int) floor(aPosY/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_x =(int) floor((aPosX+aWidth)/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_y =(int) floor((aPosY+aDepth)/reso/GROUND_MODEL_BLOCK_SIZE);
  
  //dPrint(1,"start_block_x=%d start_block_y=%d",start_block_x, start_block_y);
  //dPrint(1,"end_block_x=%d end_block_y=%d",end_block_x, end_block_y);
  
  //given data block's offset from first block's origin
  // and samples that make the end parts of row and column
  int start_offset_samples_x, end_samples_x;
  int start_offset_samples_y, end_samples_y;
  
  start_offset_samples_x=(aPosX/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_x;
  start_offset_samples_y=(aPosY/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_y;
  
  end_samples_x=GROUND_MODEL_BLOCK_SIZE*(end_block_x-start_block_x+1)-start_offset_samples_x-data_samples_x;
  end_samples_y=GROUND_MODEL_BLOCK_SIZE*(end_block_y-start_block_y+1)-start_offset_samples_y-data_samples_y;
  
  //dPrint(1,"end_samples_x=%d end_samples_y=%d",end_samples_x,end_samples_y );
  //dPrint(1,"start_offset_samples_x=%d start_offset_samples_y=%d",start_offset_samples_x,start_offset_samples_y );
  
  //take 1 row and column of vertices from next block to fill gaps between blocks
  int samples_end_x=GROUND_MODEL_BLOCK_SIZE+1;
  int samples_end_y=GROUND_MODEL_BLOCK_SIZE+1;
  int samples_start_x=start_offset_samples_x;
  int samples_start_y=start_offset_samples_y;
  
  //go through affected blocks
  for (int block_x=start_block_x; block_x <= end_block_x; block_x++)
    {
      //if last column
      if (block_x==end_block_x)
        samples_end_x=GROUND_MODEL_BLOCK_SIZE-end_samples_x;
    
      samples_start_y=start_offset_samples_y;
    
      //take 1 row and column of vertices from next block to fill gaps between blocks
      samples_end_y=GROUND_MODEL_BLOCK_SIZE+1;
    
      for (int block_y=start_block_y; block_y <= end_block_y; block_y++)
        {
          //if last row
          if (block_y==end_block_y)
            samples_end_y=GROUND_MODEL_BLOCK_SIZE-end_samples_y;
      
          TGroundModelBlock *block;
          //check if terrain exists on this point already
          if (!(block=getGroundBlock(block_x,block_y)))
            {
              //dPrint(1,"creating new block");
#warning "Using inverted y-axis and heading"
              //dPrint(1,"creating ground block x=%d y=%d",block_x,block_y);
              block=new TGroundModelBlock(GROUND_MODEL_BLOCK_SIZE*block_x*reso,
                                          -GROUND_MODEL_BLOCK_SIZE*block_y*reso,reso);
              setGroundBlock(block_x,block_y,block);
              printf("Created new ground block\n");

              //dPrint(1,"created ground block");
            }
      
          //dPrint(1,"setting samples");
          //dPrint(1,"start_offset_samples_x=%d, start_offset_samples_y=%d",start_offset_samples_x,start_offset_samples_y);
          //dPrint(1,"samples_end_x=%d, samples_end_y=%d",samples_end_x, samples_end_y);
      
          //set hm-values in every block
          for (int samples_x=samples_start_x; samples_x<samples_end_x; samples_x++)
            {
              for (int samples_y=samples_start_y; samples_y<samples_end_y; samples_y++)
                {
                  //sample position in given data
                  int data_x=(block_x-start_block_x)*GROUND_MODEL_BLOCK_SIZE
                    +samples_x-start_offset_samples_x;
                  int data_y=(block_y-start_block_y)*GROUND_MODEL_BLOCK_SIZE
                    +samples_y-start_offset_samples_y;
                  //dPrint(1,"datax=%d datay=%d data=%d",data_x, data_y, aHmData[data_y*data_samples_x+data_x]);
                  block->vertexData[(samples_y*(GROUND_MODEL_BLOCK_SIZE+1)+samples_x)*3+1]=//-0.2+1.0*rand()/RAND_MAX*0.05;
                    aHmData[data_y*data_samples_x+data_x];
          
                }
            }
          //create new trimesh
          if (block->Trimesh)
            {
              delete block->Trimesh;
            }
          block->Trimesh=new CTrimesh(block->vertexData,BLOCK_VERTEX_COUNT,
                                      block->indexData,BLOCK_INDEX_COUNT,this,SpaceID,0,material,true);
          dGeomSetPosition(block->Trimesh->GeomID,block->x,0,block->y);
      
          samples_start_y=0;
        }
      samples_start_x=0;
    }
  
}

void CTerrain::getGroundModel(const float aPosX, const float aPosY, 
                              const int data_samples_x, const int data_samples_y,
                              float* aHmData)
{
  float aWidth=data_samples_x*reso; 
  float aDepth=data_samples_y*reso;
  
  int start_block_x =(int) floor(aPosX/reso/GROUND_MODEL_BLOCK_SIZE);
  int start_block_y =(int) floor(aPosY/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_x =(int) floor((aPosX+aWidth)/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_y =(int) floor((aPosY+aDepth)/reso/GROUND_MODEL_BLOCK_SIZE);
  
  int start_offset_samples_x, end_samples_x;
  int start_offset_samples_y, end_samples_y;
  
  start_offset_samples_x=(aPosX/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_x;
  start_offset_samples_y=(aPosY/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_y;
  
  end_samples_x=GROUND_MODEL_BLOCK_SIZE*(end_block_x-start_block_x+1)-start_offset_samples_x-data_samples_x;
  end_samples_y=GROUND_MODEL_BLOCK_SIZE*(end_block_y-start_block_y+1)-start_offset_samples_y-data_samples_y;
  
  int samples_end_x=GROUND_MODEL_BLOCK_SIZE;
  int samples_end_y=GROUND_MODEL_BLOCK_SIZE;
  int samples_start_x=start_offset_samples_x;
  int samples_start_y=start_offset_samples_y;
  
  //go through affected blocks
  for (int block_x=start_block_x; block_x <= end_block_x; block_x++)
    {
      //if last column
      if (block_x==end_block_x)
        samples_end_x=GROUND_MODEL_BLOCK_SIZE-end_samples_x;
    
      samples_start_y=start_offset_samples_y;
    
      samples_end_y=GROUND_MODEL_BLOCK_SIZE;
    
      for (int block_y=start_block_y; block_y <= end_block_y; block_y++)
        {
          //if last row
          if (block_y==end_block_y)
            samples_end_y=GROUND_MODEL_BLOCK_SIZE-end_samples_y;
      
          TGroundModelBlock *block;
          //check if terrain exists on this point already
          block=getGroundBlock(block_x,block_y);
      
          //set hm-values in every block
          for (int samples_x=samples_start_x; samples_x<samples_end_x; samples_x++)
            {
              for (int samples_y=samples_start_y; samples_y<samples_end_y; samples_y++)
                {
                  //sample position in given data
                  int data_x=(block_x-start_block_x)*GROUND_MODEL_BLOCK_SIZE
                    +samples_x-start_offset_samples_x;
                  int data_y=(block_y-start_block_y)*GROUND_MODEL_BLOCK_SIZE
                    +samples_y-start_offset_samples_y;
          
                  if (block)
                    {
                      aHmData[data_y*data_samples_x+data_x]=
                        block->vertexData[(samples_y*(GROUND_MODEL_BLOCK_SIZE+1)+samples_x)*3+1];
                    } else {
                      aHmData[data_y*data_samples_x+data_x]=0;
                    }
                }
            }
      
          samples_start_y=0;
        }
      samples_start_x=0;
    }
  
}

#ifndef NO_MAP
void CTerrain::updateMap(MaCI::Map::CMapData &mapData)
{
  const MaCI::Map::TGridBlockHeader *block;
  const MaCI::Map::TMapUpdateHeader *header;
  const float * data;
  
  header=mapData.GetMapUpdateHeader();
  
  if (header)
    {
      if (header->clear)
        {
          clearBlocks();
          clearAddedWalls();
        }
    }
  
  for (int i=0;i<mapData.GetGridBlockCount();i++)
    {
      block=mapData.GetGridBlockHeader(i);
      if (block->dataType==MaCI::Map::KGridTypeHeightMap) //uses only hm for now
        {
      
          data = (const float*) mapData.GetGridBlockData(i);
          //updateGroundModel(block->x,block->y,block->width,block->height,
          //header->resolution,data);
          updateGroundModel(block->x,block->y,block->samples_x,block->samples_y,
                            hmResolution,data);
        }
    }
  
  const MaCI::Map::TMapLine *line;
          
  for (int i=0;i<mapData.GetMapLineCount();i++)
    {
      line=mapData.GetMapLine(i);
      dPrint(1,"received map line, x1=%f y1=%f x2=%f y2=%f",line->x1,line->y1,line->x2,line->y2);
      CWall *w=new CWall(SimWorld,line->x1,line->y1,line->x2,line->y2,SimWorld->wallWidth,
                         SimWorld->wallHeight);
      dPrint(1,"Wall created");
      SimWorld->InsertObject(w);
      addedWalls.push_back(w);
    }
  
  //dPrint(1,"done updating map");
  
}
#endif

void CTerrain::connectGIMI()
{

  // We connect to gimi the "correct" way
  SimWorld->ConnectGimi(&gimi, name);

  // We connect the mapClient
  mapClient.SetDefaultTarget(name);
  mapClient.Open();

  /**
  if (gimi.readSettings("gimi_settings.xml"))
    {
      int r;
#if defined (USE_MACI) and ! defined(NO_MAP)
      r=gimi.connectToHubEx("",-1);
#elif ! defined(NO_MAP)
      r=gimi.connectToHubEx("",-1,name);
#endif
      if (r==GIMI_OK)
        {
          dPrint(1,"Connected using name %s",name.c_str());
#if defined (USE_MACI) and ! defined(NO_MAP)
          MaCI::EMaCIError e;
          mapClient.SetDefaultTarget(name);
          e=mapClient.Open();
          if (e!=MaCI::KMaCIOK)
            {
              dPrint(1,"Couldnt open map client");
            }
#elif ! defined(NO_MAP)
          gimi.addAcceptedService(GIMI_PROTOCOL_GRID,GIMI_IGNOREMINORTYPE,"Ground model");
#endif      
        } else if (r==GIMI_INVALIDNAME){
          dPrint(1,"Couldn't connect GIMI: Name '%s' already in use.", name.c_str());
        } else {
          dPrint(1,"Couldn't connect GIMI: GIMI_ERROR.");
        }

    } else {
      dPrint(1,"Couldn't read connection settings file (gimi_settings.xml).");
    }
*/
  
}

TGroundModelBlock* CTerrain::getGroundBlock(int x, int y)
{
  gmbMapIter i;
  if ((i=groundBlocks.find(TGmbKey(x,y)))==groundBlocks.end())
    {  
      return NULL;
    } else { 
      return i->second;
    }
}

void CTerrain::setGroundBlock(int x, int y, TGroundModelBlock* block)
{
  dPrint(1,"inserting...");
  groundBlocks.insert(gmbPair(TGmbKey(x,y), block));
  dPrint(1,"done");
}

void CTerrain::clearBlocks()
{
  gmbMapIter i;
  for (i=groundBlocks.begin();i!=groundBlocks.end();i++)
    delete i->second;
  groundBlocks.clear();
}

void CTerrain::clearAddedWalls()
{
  std::vector<CWall*>::iterator i;
  
  for (i=addedWalls.begin();i!=addedWalls.end();i++)
    SimWorld->RemoveObject(*i);
  
  addedWalls.clear();
}

bool CTerrain::IsTerrain(CPrimitive *primitive)
{
  if (primitive==Plane)
    {
      //dPrint(1,"plane touching");
      return true;
    }
    
  if (type==TERRAIN_TYPE_HMAP)
    {
      gmbMapIter i;
      for (i=groundBlocks.begin();i!=groundBlocks.end();i++)
        {
          if (i->second->Trimesh==primitive)
            return true;
        }
    }
  
  return false;

}

void CTerrain::updateTerrain(const float aPosX, const float aPosY, 
                             const int data_samples_x, const int data_samples_y,
                             float* aHmData)
{
  TUpdate update;
  
  update.x=aPosX;
  update.y=aPosY;
  
  update.samples_x=data_samples_x;
  update.samples_y=data_samples_y;
  

  update.data=new float[data_samples_x*data_samples_y];

  memcpy((char*)update.data,(char*)aHmData,data_samples_x*data_samples_y*sizeof(float));
  
  updates.push_back(update);
  //dPrint(1,"pushed update to vector");
  
}
